#pragma once

#include "vio/front_end/camera.h"
#include "vio/front_end/feature.h"
#include "vio/front_end/frame.h"
#include "vio/front_end/map.h"
#include "vio/front_end/patch_score.h"
#include "vio/front_end/point.h"
#include "vio/tools/math_utils.h"
#include "vio/tools/vision_utils.h"
#include "vio/vio_common.h"

#include "vio/front_end/feature_alignment.h"

namespace vio {

// warp a patch from the reference view to the current view.
namespace warp {

/**
 * ref
 * https://www.ri.cmu.edu/pub_files/pub3/baker_simon_2002_3/baker_simon_2002_3.pdf
 * https://www.robots.ox.ac.uk/~lav/Papers/molton_etal_bmvc2004/molton_etal_bmvc2004.pdf
 * http://alumni.media.mit.edu/~maov/classes/comp_photo_vision08f/lect/08_image_warps.pdf
 * https://docs.opencv.org/3.4/d4/d61/tutorial_warp_affine.html
 */
void GetWarpMatrixAffine(const Camera::Ptr cam_ref,
                         const Camera::Ptr cam_cur,
                         const Eigen::Vector2d& px_ref,
                         const Eigen::Vector3d& f_ref,
                         const double& depth_ref,
                         const Sophus::SE3d& T_cur_ref,
                         const int level_ref,
                         Eigen::Matrix2d& A_cur_ref);

int GetBestSearchLevel(const Eigen::Matrix2d& A_cur_ref, const int max_level);

void WarpAffine(const Eigen::Matrix2d& A_cur_ref,
                const cv::Mat& img_ref,
                const Eigen::Vector2d& px_ref,
                const int level_ref,
                const int level_cur,
                const int half_patch_size,
                uint8_t* patch_ptr);
}  // namespace warp

// patch-matcher for reprojection-matching and epipolar search in triangulation
class PatchMatcher {
 public:
  EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
  typedef std::shared_ptr<PatchMatcher> Ptr;
  static const int half_patch_size_ = 4;
  static const int patch_size_ = 8;

  typedef patch_score::ZMSSD<half_patch_size_> PatchScore;

  struct Options {
    bool align_1d;  // in epipolar search: align patch 1D along epipolar line
    int align_max_iter;  // number of iterations for aligning the feature
                         // patches in gauss newton
    double max_epi_length_optim;  // max length of epipolar line to skip epiplar
                                  // search and directly go to image align
    bool subpix_refinement;  // do gauss newton feature patch alignment after
                             // epipolar search
    size_t
        max_epi_search_steps;  // max number of evaluations along epipolar line
    bool epi_search_edgelet_filtering;
    double epi_search_edgelet_max_angle;
    Options()
        : align_1d(false),
          align_max_iter(10),
          max_epi_length_optim(2.0),
          max_epi_search_steps(1000),
          subpix_refinement(true),
          epi_search_edgelet_filtering(true),
          epi_search_edgelet_max_angle(0.7) {}
  } options_;

  uint8_t patch_[patch_size_ * patch_size_] __attribute__((aligned(16)));
  uint8_t patch_with_border_[(patch_size_ + 2) * (patch_size_ + 2)]
      __attribute__((aligned(16)));
  Eigen::Matrix2d A_cur_ref_;  // affine warp matrix
  Eigen::Vector2d epi_dir_;
  double epi_length_;  // length of epipolar line segment in pixels (only used
                       // for epipolar search)
  double h_inv_;       // hessian of 1D image aligment along epipolar line
  int search_level_;
  bool reject_;
  Feature::Ptr ref_ftr_;
  Eigen::Vector2d px_cur_;

  PatchMatcher() = default;

  // find a match by directly applying subpix refinement.
  // IMPORTANT! this function assumes that px_cur is already set to an
  // estimate that is within ~2~3 pixel of the final result!
  bool FindMatchDirect(const Point& point,
                       const Frame& curr_frame,
                       Eigen::Vector2d& px_cur);

  // find a match by searching along the epipolar line without using any
  // features
  bool FindEpipolarMatchDirect(const Frame& ref_frame,
                               const Frame& cur_frame,
                               const Feature& ref_ftr,
                               const double& d_estimate,
                               const double& d_min,
                               const double& d_max,
                               double& depth);

  void CreatePatchFromPatchWithBorder();
};

}  // namespace vio
